
#include "vector3.h"

void vector3_create(float x, float y, float z, struct vector3* v_out)
{
	v_out->x = x;
	v_out->y = y;
	v_out->z = z;
}
void vector3_create_single(float value, struct vector3* v_out)
{
	float i = value;
	v_out->x = i;
	v_out->y = i;
	v_out->z = i;
}
void vector3_create_vector2(const struct vector2* v, float z, struct vector3* v_out)
{
	v_out->x = v->x;
	v_out->y = v->y;
	v_out->z = z;
}
void vector3_add(const struct vector3* v1, const struct vector3* v2, struct vector3* v_out)
{
	v_out->x = v1->x + v2->x;
	v_out->y = v1->y + v2->y;
	v_out->z = v1->z + v2->z;
}
void vector3_barycentric(const struct vector3* v1, const struct vector3* v2, const struct vector3* v3, float amt1, float amt2, struct vector3* v_out)
{
	v_out->x = v1->x + amt1 * (v2->x - v1->x) + amt2 * (v3->x - v1->x);
	v_out->y = v1->y + amt1 * (v2->y - v1->y) + amt2 * (v3->y - v1->y);
	v_out->z = v1->z + amt1 * (v2->z - v1->z) + amt2 * (v3->z - v1->z);
}
void vector3_catmullrom(const struct vector3* v1, const struct vector3* v2, const struct vector3* v3, 
						const struct vector3* v4, float amt, struct vector3* v_out)	
{
	float single = amt * amt;
	float single1 = amt * single;
	v_out->x = 0.5f * (2.0f * v2->x + (-v1->x + v3->x) * amt + (2.0f * v1->x - 5.0f * v2->x + 4.0f * v3->x - v4->x) * single + (-v1->x + 3.0f * v2->x - 3.0f * v3->x + v4->x) * single1);
	v_out->y = 0.5f * (2.0f * v2->y + (-v1->y + v3->y) * amt + (2.0f * v1->y - 5.0f * v2->y + 4.0f * v3->y - v4->y) * single + (-v1->y + 3.0f * v2->y - 3.0f * v3->y + v4->y) * single1);
	v_out->z = 0.5f * (2.0f * v2->z + (-v1->z + v3->z) * amt + (2.0f * v1->z - 5.0f * v2->z + 4.0f * v3->z - v4->z) * single + (-v1->z + 3.0f * v2->z - 3.0f * v3->z + v4->z) * single1);
}
void vector3_clamp(const struct vector3* v_in, const struct vector3* min, const struct vector3* max, struct vector3* v_out)
{
	float x, single, single1, y, y1, z, z1, x1, z2;
	x1 = v_in->x;

	if (x1 > max->x)
	{
		x = max->x;
	}
	else
	{
		x = x1;
	}
	x1 = x;
	if (x1 < min->x)
	{
		single = min->x;
	}
	else
	{
		single = x1;
	}
	x1 = single;
	single1 = v_in->y;

	if (single1 > max->y)
	{
		y = max->y;
	}
	else
	{
		y = single1;
	}
	single1 = y;
	if (single1 < min->y)
	{
		y1 = min->y;
	}
	else
	{
		y1 = single1;
	}
	single1 = y1;
	
	z2 = v_in->z;
	if (z2 > max->z)
	{
		z = max->z;
	}
	else
	{
		z = z2;
	}
	z2 = z;
	if (z2 < min->z)
	{
		z1 = min->z;
	}
	else
	{
		z1 = z2;
	}
	z2 = z1;
	v_out->x = x1;
	v_out->y = single1;
	v_out->z = z2;
}
void vector3_cross(const struct vector3* v1, const struct vector3* v2, struct vector3* v_out)
{
	float x = v1->x * v2->y - v1->y * v2->x;
	float y = v1->y * v2->z - v1->z * v2->y;
	float z = v1->z * v2->x - v1->x * v2->z;
	v_out->x = y;
	v_out->y = z;
	v_out->z = x;
}
void vector3_dist(const struct vector3* v1, const struct vector3* v2, float* f_out)
{
	float x = v1->x - v2->x;
	float y = v1->y - v2->y;
	float z = v1->z - v2->z;
	float single = x * x + y * y + z * z;
	*f_out = sqrtf(single);
}
void vector3_distsqr(const struct vector3* v1, const struct vector3* v2, float* f_out)
{
	float x = v1->x - v2->x;
	float y = v1->y - v2->y;
	float z = v1->z - v2->z;
	*f_out = x * x + y * y + z * z;
}
void vector3_div(const struct vector3* v1, const struct vector3* v2, struct vector3* v_out)
{
	v_out->x = v1->x / v2->x;
	v_out->y = v1->y / v2->y;
	v_out->z = v1->z / v2->z;
}
void vector3_divf(const struct vector3* v, float n, struct vector3* v_out)
{
	float temp = 1.0f / n;
	v_out->x = v->x * temp;
	v_out->y = v->y * temp;
	v_out->z = v->z * temp;
}
void vector3_dot(const struct vector3* v1, const struct vector3* v2, float* f_out)
{
	*f_out = v1->x * v2->x + v1->y * v2->y + v1->z * v2->z;
}
void vector3_equals(const struct vector3* v1, const struct vector3* v2, int* result)
{
	if (v1->x != v2->x)
	{
		*result = 0;
		return;
	}
	if (v1->y != v2->y)
	{
		*result = 0;
		return;
	}
	if (v1->z != v2->z)
	{
		*result = 0;
		return;
	}
	*result = 1;
}
void vector3_hermite(const struct vector3* v1, const struct vector3* tangent1, const struct vector3* v2, const struct vector3* tangent2, float amt, struct vector3* v_out)
{
	float single = amt * amt;
	float single1 = amt * single;
	float single2 = 2.0f * single1 - 3.0f * single + 1.0f;
	float single3 = -2.0f * single1 + 3.0f * single;
	float single4 = single1 - 2.0f * single + amt;
	float single5 = single1 - single;

	v_out->x = v1->x * single2 + v2->x * single3 + tangent1->x * single4 + tangent2->x * single5;
	v_out->y = v1->y * single2 + v2->y * single3 + tangent1->y * single4 + tangent2->y * single5;
	v_out->z = v1->z * single2 + v2->z * single3 + tangent1->z * single4 + tangent2->z * single5;
}
void vector3_len(const struct vector3* v, float* f_out)
{
	*f_out = sqrtf(v->x * v->x + v->y * v->y + v->z * v->z);
}
void vector3_lensqr(const struct vector3* v, float* f_out)
{
	*f_out = v->x * v->x + v->y * v->y + v->z * v->z;
}
void vector3_lerp(const struct vector3* v1, const struct vector3* v2, float amt, struct vector3* v_out)
{
	v_out->x = v1->x + (v2->x - v1->x) * amt;
	v_out->y = v1->y + (v2->y - v1->y) * amt;
	v_out->z = v1->z + (v2->z - v1->z) * amt;
}
void vector3_max(const struct vector3* v1, const struct vector3* v2, struct vector3* v_out)
{	
	if (v1->x > v2->x)
	{
		v_out->x = v1->x;
	}
	else
	{
		v_out->x = v2->x;
	}
	if (v1->y > v2->y)
	{
		v_out->y = v1->y;
	}
	else
	{
		v_out->y = v2->y;
	}
	if (v1->z > v2->z)
	{
		v_out->z = v1->z;
	}
	else
	{
		v_out->z = v2->z;
	}
}
void vector3_min(const struct vector3* v1, const struct vector3* v2, struct vector3* v_out)
{
	if (v1->x < v2->x)
	{
		v_out->x = v1->x;
	}
	else
	{
		v_out->x = v2->x;
	}
	if (v1->y < v2->y)
	{
		v_out->y = v1->y;
	}
	else
	{
		v_out->y = v2->y;
	}
	if (v1->z < v2->z)
	{
		v_out->z = v1->z;
	}
	else
	{
		v_out->z = v2->z;
	}
}
void vector3_mult(const struct vector3* v1, const struct vector3* v2, struct vector3* v_out)
{
	v_out->x = v1->x * v2->x;
	v_out->y = v1->y * v2->y;
	v_out->z = v1->z * v2->z;
}
void vector3_multf(const struct vector3* v, float scale, struct vector3* v_out)
{
	v_out->x = v->x * scale;
	v_out->y = v->y * scale;
	v_out->z = v->z * scale;
}
void vector3_negate(const struct vector3* v, struct vector3* v_out)
{
	v_out->x = -v->x;
	v_out->y = -v->y;
	v_out->z = -v->z;
}
void vector3_normalize(const struct vector3* v, struct vector3* v_out)
{
	float x = v->x * v->x + v->y * v->y + v->z * v->z;
	float temp = 1.0f / sqrtf(x);

	v_out->x = v->x * temp;
	v_out->y = v->y * temp;
	v_out->z = v->z * temp;
}
void vector3_reflect(const struct vector3* v, const struct vector3* normal, struct vector3* v_out)
{
	float x = v->x * normal->x + v->y * normal->y + v->z * normal->z;

	v_out->x = v->x - 2.0f * x * normal->x;
	v_out->y = v->y - 2.0f * x * normal->y;
	v_out->z = v->z - 2.0f * x * normal->z;
}
void vector3_smoothstep(const struct vector3* v1, const struct vector3* v2, float amt, struct vector3* v_out)
{
	float single;

	if (amt > 1.0f)
	{
		single = 1.0f;
	}
	else
	{
		if (amt < 0.0f)
		{
			single = 0.0f;
		}
		else
		{
			single = amt;
		}
	}
	amt = single;
	amt = amt * amt * (3.0f - 2.0f * amt);

	v_out->x = v1->x + (v2->x - v1->x) * amt;
	v_out->y = v1->y + (v2->y - v1->y) * amt;
	v_out->z = v1->z + (v2->z - v1->z) * amt;
}
void vector3_subt(const struct vector3* v1, const struct vector3* v2, struct vector3* v_out)
{
	v_out->x = v1->x - v2->x;
	v_out->y = v1->y - v2->y;
	v_out->z = v1->z - v2->z;
}
void vector3_transform_matrix4(const struct vector3* pos, const struct matrix4* matrix, struct vector3* v_out)
{
	v_out->x = pos->x * matrix->m11 + pos->y * matrix->m21 + pos->z * matrix->m31 + matrix->m41;
	v_out->y = pos->x * matrix->m12 + pos->y * matrix->m22 + pos->z * matrix->m32 + matrix->m42;
	v_out->z = pos->x * matrix->m13 + pos->y * matrix->m23 + pos->z * matrix->m33 + matrix->m43;
}
void vector3_transform_quaternion(const struct vector3* v, const struct quaternion* rotation, struct vector3* v_out)
{
	float x = rotation->x + rotation->x;
	float y = rotation->y + rotation->y;
	float z = rotation->z + rotation->z;
	float w = rotation->w * x;
	float single = rotation->w * y;
	float w1 = rotation->w * z;
	float x1 = rotation->x * x;
	float single1 = rotation->x * y;
	float x2 = rotation->x * z;
	float y1 = rotation->y * y;
	float y2 = rotation->y * z;
	float z1 = rotation->z * z;
	float single2 = v->x * (1.0f - y1 - z1) + v->y * (single1 - w1) + v->z * (x2 + single);
	float x3 = v->x * (single1 + w1) + v->y * (1.0f - x1 - z1) + v->z * (y2 - w);
	float single3 = v->x * (x2 - single) + v->y * (y2 + w) + v->z * (1.0f - x1 - y1);

	v_out->x = single2;
	v_out->y = x3;
	v_out->z = single3;
}
void vector3_transformnormal(const struct vector3* normal, const struct matrix4* matrix, struct vector3* v_out)
{
	v_out->x = normal->x * matrix->m11 + normal->y * matrix->m21 + normal->z * matrix->m31;
	v_out->y = normal->x * matrix->m12 + normal->y * matrix->m22 + normal->z * matrix->m32;
	v_out->z = normal->x * matrix->m13 + normal->y * matrix->m23 + normal->z * matrix->m33;
}